//!!! This source file is for Arduino MEGA!!!
/***************************************************************/
/* Humidity sensor *********************************************/
/*  0 - Air
    1 - 300 Dry Soil
    300 - 700 Wet Soil
    700 - 950 Water
*/
int sensorPin = A0;     // select the input pin for the sensor
int ledPin = 13;        // select the pin for the LED
#define SENSOR_READ_NUMBER 50
int readValues[SENSOR_READ_NUMBER];

void initSoilSensor()
{
  pinMode(ledPin, OUTPUT);
  pinMode(sensorPin, INPUT);
  digitalWrite(ledPin, LOW);
}

int medieSensor()
{
  int result=0;
  for (int i=0; i<SENSOR_READ_NUMBER; i++)
  {
    result += readValues[i];
  }
  result /= SENSOR_READ_NUMBER;
  return (int)result;
}

int handlerSoilSensor()
{
  // read the value from the sensor
  int sensorValue = analogRead(sensorPin);
  // turn the ledPin on
  digitalWrite(ledPin, HIGH);
  // show that reading was made
  delay(500);
  digitalWrite(ledPin, LOW);
  return sensorValue;
}

byte sendHumAndLocation(int value, int location)
{
 /*
 MOTOR 1                 MOTOR 2                MOTOR 3
 0b0000 ->Very Dry       0b0100 ->Very Dry      0b1000 ->Very Dry
 0b0001 ->Dry            0b0101 ->Dry           0b1001 ->Dry
 0b0010 ->Medium         0b0110 ->Medium        0b1010 ->Medium
 0b0011 ->Wet            0b0111 ->Wet           0b1011 ->Wet
 
 we send via bluetooth only a byte to reduce the sending error.
 */
  
  #define SOIL_DRY      200
  #define SOIL_MEDIUM   450
  #define SOIL_WET      650
  
  byte result = 0;
  
  if (value < SOIL_DRY)
    result = 0b00;
  else
    if (value < SOIL_MEDIUM)
      result = 0b01;
    else
      if (value < SOIL_WET)
        result = 0b10;
      else
        if (value >= SOIL_WET)
          result = 0b11;
 
   switch (location)
   {
     case 1: result = (result << 2) | 0b0000;
       break;
     case 2: result = (result << 2) | 0b0001;
       break;
     case 3: result = (result << 2) | 0b0010;
       break;
   }
  return result;
}

/***************************************************************/
/* Robot Arm ***************************************************/
/*
  value 0 -> 0 degree
  value 1500 -> 90 degree
  value 3000 -> 180 de degree
*/

#define SERVO_BASE_HORIZONTAL 0
#define SERVO_BASE_VERTICAL   1
#define SERVO_MID_VERTICAL    2
#define SERVO_TOP_VERTICAL    3

void initArm()
{
  Serial2.begin(9600);
}

void moveArm(int servoNb, int position, int time)
{
  Serial2.print("#");
  Serial2.print(servoNb);
  Serial2.print(" P");
  Serial2.print(position);
  Serial2.print(" T");
  Serial2.println(time);
  delay(time);
}

/***************************************************************/
/*Bluetooth ****************************************************/
/*  This part is the MASTER Bluetooth and is HW configured as Master

*/
void initBluetooth()
{
    Serial1.begin(9600);
}

/***************************************************************/
/* Main variables */

int signalEnablePin = 3; // select the pin for signaling that we
                         // can start moving the Robotic Arm.
int signalDisablePin = 4;// select the pin for signaling that we
                         // can finnished moving the Robotic Arm.
#define STATUS_POS_ARM_IDLE        0
#define STATUS_POS_ARM_BEFORE_READ 1
#define STATUS_POS_ARM_READING     2
#define STATUS_POS_ARM_AFTER_READ  3

int statusPosArm = STATUS_POS_ARM_IDLE;

void setup() {
  
  initSoilSensor();
  initArm();
  initBluetooth();
  
  /* Set the enable pin as input */
  pinMode(signalEnablePin, INPUT);
  /* Set the disable pin as output */
  pinMode(signalDisablePin, OUTPUT);
}

void loop() {
  static int locationNumber = 0;  // the lcation we are now 
  switch(statusPosArm)
  {
    case STATUS_POS_ARM_IDLE:
    {
      if( digitalRead(signalEnablePin) )
      {
        // we can start the process
        //if(locationNumber > 0)
        statusPosArm = STATUS_POS_ARM_BEFORE_READ;
        locationNumber++;
        // start sending the signal to Robot to signal we start operation
        digitalWrite(signalDisablePin, HIGH);
      }
      else
      {
        // Robot is still positioning, wait for signal to start
        if(locationNumber >= 3)
        {
          // we are done with measurements, power off Robotic Arm.
          delay(100);
        }
        else
        {
          // wait for Robot to position to the next location
          delay(100);
        }
      }
    }
    break;
    case STATUS_POS_ARM_BEFORE_READ:
    {
      moveArm(SERVO_BASE_VERTICAL, 1600, 1000);
      moveArm(SERVO_BASE_HORIZONTAL, 1500, 1000);
      moveArm(SERVO_BASE_VERTICAL, 1600, 1000);
      moveArm(SERVO_MID_VERTICAL, 1500, 750);
      moveArm(SERVO_TOP_VERTICAL, 950, 750);
      moveArm(SERVO_BASE_VERTICAL, 800, 750);
      delay(1000);
      statusPosArm = STATUS_POS_ARM_READING;
    }
    break;
    case STATUS_POS_ARM_READING:
    {
      byte valueToSend = 0;
      for(int i=0; i<SENSOR_READ_NUMBER; i++)
      {
        readValues[i] = handlerSoilSensor();
      }
      readValues[SENSOR_READ_NUMBER] = medieSensor();
      // Figure out how much water we need and sent for that location
      valueToSend = sendHumAndLocation(readValues[SENSOR_READ_NUMBER], locationNumber);
      Serial1.write(valueToSend);
      delay(1000);
      statusPosArm = STATUS_POS_ARM_AFTER_READ;
    }
    break;
    case STATUS_POS_ARM_AFTER_READ:
    {
      moveArm(SERVO_BASE_VERTICAL, 1600, 1000);
      moveArm(SERVO_BASE_HORIZONTAL, 300, 1000);
      moveArm(SERVO_BASE_VERTICAL, 2000, 1000);
      delay(10000);
      // we are done with this mesurment move to another
      statusPosArm = STATUS_POS_ARM_IDLE;
      // sent to Robot signal that he can move.
      digitalWrite(signalDisablePin, LOW);      
    }
    break;
  }
}
